#include "tube_planning/polynomial.h"
#include <ros/ros.h>


namespace Tube_planning
{
    Polynomial::Polynomial(const Eigen::MatrixXd &Polys, const Eigen::ArrayXd& ts)
    {
        Polynomial::setPolynomial(Polys, ts);
    }

    Polynomial::~Polynomial(){}

    void Polynomial::setPolynomial(const Eigen::MatrixXd &Polys, const Eigen::ArrayXd& ts)
    {
        Poly_mat = Polys;
        n_segment = Polys.cols();
        n_order = Polys.rows() - 1;
        n_coef = Polys.rows();
        tt = ts;
        start_time = ts(0);
        end_time = ts(n_segment);
    }

    double Polynomial::getTotalTime()
    {
        return end_time - start_time;
    }

    double Polynomial::evaluatePolynomial(const double t, const int r)
    {
        double t_fit = t;
        if (t >= end_time)
        {
            t_fit = end_time;
        }
        if (t < start_time)
        {
            t_fit = start_time;
        }

        idx = 0;
        while (idx < tt.size() - 1 && t_fit > tt(idx + 1) + 0.0001)
        {
            idx = idx + 1;
        }

        return poly_val(t_fit, r);
    }

    Eigen::ArrayXd Polynomial::evaluatePolynomialSequence(const Eigen::ArrayXd& t_seq, const int r)
    {
        // 输入：时间序列 阶数
        // 输出：多项式值的序列
        int N = t_seq.size();
        Eigen::ArrayXd vals = Eigen::ArrayXd::Zero(N);
        for (int i = 0; i < N; i++)
        {
            double t = t_seq(i);
            vals(i) = evaluatePolynomial(t, r);
        }
        return vals;
    }

    double Polynomial::poly_val(const double t, const int r)
    {
        // 输入：分段多项式中某一段的系数 时刻 阶数
        // 输出：对应的多项式值
        double val = 0;
        Eigen::ArrayXd poly = Poly_mat.block(0, idx, n_coef, 1);
        int n = poly.size() - 1;
        if (r <= 0)
        {
            for (int i = 0; i <= n; i++)
            {
                val = val + poly(i) * pow(t, i);
            }
        }
        else
        {
            for (int i = r; i <= n; i++)
            {
                double a = poly(i) * std::tgamma(i + 1) / std::tgamma(i - r + 1) * pow(t, i - r);
                val = val + a;
            }
        }
        return val;
    }
    
}